#include <iostream>
#include <time.h>
#include <signal.h>
#include <string.h>
#include <unistd.h>
#include "libmodbus/modbus.h"
#include "libmodbus/modbus-rtu.h"
#include "libmodbus/modbus-tcp.h"
#include "jsonconfig/jsonconfig.h"
#include "rtubuffer/rtubuffer.h"
#include "modbusserver/modbusserver.h"
#include <vector>
#include <queue>
#include <thread>
#include <mutex>
#include <ctime>

using namespace std;
mutex response_mutex;
struct ResponseDataType responsedata;
mutex hpcmd_mutex;
struct RequestDataType highprioritycmd;
mutex buf_mutex;
struct
{
    RequestCode code; /*<当前执行的指令*/
    bool feedback;    /*<是否需要回传*/
} currenttask;
struct
{
    int request = 0;
    int polling = 0;
} timeout;
extern std::vector<PollingCode> pollingqueue;
modbus_t *rtu_handler = NULL;

static void log(string msg, int level)
{
    if (!config.debuglog)
        return;
    if (level == 0 && level >= config.loglevel)
        cout << "\033[36m[main]\033[0m" << msg << endl;
    else if (level == 1 && level >= config.loglevel)
        cout << "\033[36m[main]\033[0m"
             << "\033[36m" << msg << "\033[0m" << endl;
    else if (level >= 2)
        cout << "\033[36m[main]\033[0m"
             << "\033[31m" << msg << "\033[0m" << endl;
}

void timecheck(int cmd)
{
    static struct timeval t1,t2;
    if(!config.debuglog)
        return;
    if(cmd == 1)
        gettimeofday(&t1,NULL);
    else if(cmd == 2)
        gettimeofday(&t2,NULL);
    int time_spend = (t2.tv_sec - t1.tv_sec)*1000 + 
            (t2.tv_usec - t1.tv_usec)/1000;
    if(time_spend > 0)
        printf("Communication Time Cost : %d ms\n",time_spend);
}

/**
 * @brief 初始化ModebusRTU与TCP
 * 
 * @return true 无误
 * @return false 失败
 */
bool ModbusBridge_init()
{
    if(!BridgeJsonConfig("bridgeconfig.json"))
        exit(-1);
    auto rtu = config.rtuconfig;
    rtu_handler = modbus_new_rtu(rtu.rtuport.c_str(), rtu.baudrate, rtu.parity, rtu.databit, rtu.stopbit);
    if (rtu_handler == NULL)
    {
        log("Failed to creat RTU Master!", 2);
        return false;
    }
    modbus_set_debug(rtu_handler, 0);
    int s = config.bridgeconfig.requesttimeout / 1000;
    int ms = config.bridgeconfig.requesttimeout % 1000;
    modbus_set_response_timeout(rtu_handler, s, ms * 1000);
    if(modbus_connect(rtu_handler) == -1)
        return false;
    init_buffer();
    return true;
}

/**
 * @brief ModbusTCP线程
 * 
 * @param cfg config副本
 */
void serverThread(BridgeConfig *cfg)
{
    modbusServer server(cfg);
    server.exec();
}

int main()
{
    RequestDataType h_cmd;      //高优先级透传指令
    struct timeval last_time,current_time;
    int time_spend = 0;
    if (!ModbusBridge_init())
        exit(-1);
    int tick_ms = config.bridgeconfig.looptimeout;
    //建立ModbusTCP Server线程
    thread modSerThread(serverThread, &config);
    modSerThread.detach();
    uint16_t data[125];         //125为modbus最多传输寄存器数目
    uint16_t wdata[125];
    int rc;
    bool send = false;          //发送RTUrequest
    bool miss = false;          //等待回复timeout
    auto iter = pollingqueue.begin();
    h_cmd.enable = false;
    timeout.polling = 0;
    timeout.request = 0;
    //轮询处理ModbusRTU
    gettimeofday(&last_time,NULL);
    while (true)
    {
        //STEP0:计时
        gettimeofday(&current_time,NULL);
        time_spend = (current_time.tv_sec - last_time.tv_sec)*1000 + 
            (current_time.tv_usec - last_time.tv_usec)/1000;
        if(time_spend < tick_ms)
            time_spend = tick_ms;
        last_time = current_time;
        timeout.polling = (timeout.polling < time_spend) ? 0 : (timeout.polling - time_spend);
        timeout.request = (timeout.request < time_spend) ? 0 : (timeout.request - time_spend);
        //STEP1:检查有无转发消息需要处理
        if(!h_cmd.enable)
        {
            hpcmd_mutex.lock();
            if(highprioritycmd.enable)
            {
                h_cmd = highprioritycmd;
                if(h_cmd.cmd.func == 0x10)
                {
                    for(int i = 0;i<h_cmd.cmd.nb;i++)
                        wdata[i] = highprioritycmd.req_data[i];
                }
                highprioritycmd.enable = false;
            }
            hpcmd_mutex.unlock();
        }
        //STEP2：处理RTU转发
        if (!timeout.request && h_cmd.enable)
        { //最小间隔后即可发送高优先级消息
            log("request income", 1);
            h_cmd.enable = false;
            currenttask.code = h_cmd.cmd;
            currenttask.feedback = true;
            send = true;
            timeout.request = config.bridgeconfig.commtimeout;
        }
        else if ((!timeout.request) && (!timeout.polling))
        {   //进行轮询
            static uint8_t last_id = 0,slave_cnt = 0;;
            currenttask.code = *iter;
            currenttask.feedback = false;
            send = true;
            if(last_id != currenttask.code.slave)
            {
                slave_cnt = 1;
                last_id = currenttask.code.slave;
            }
            else
            {
                slave_cnt++;
            }
            iter++;
            if(iter == pollingqueue.end())
            {
                iter = pollingqueue.begin();
            }
            timeout.request = config.bridgeconfig.commtimeout;
            timeout.polling = config.bridgeconfig.pollingtimeout;
        }
        //处理发送任务
        if (send)
        {
            uint8_t slave = currenttask.code.slave;
            uint8_t func = currenttask.code.func;
            uint16_t addr = currenttask.code.addr;
            uint16_t nb = currenttask.code.nb;
            modbus_set_slave(rtu_handler, slave);
            if (func == 0x03 || func == 0x04)
            {
                timecheck(1);
                if (func == 0x03)
                    rc = modbus_read_registers(rtu_handler, addr, nb, data);
                else
                    rc = modbus_read_input_registers(rtu_handler, addr, nb, data);
                timecheck(2);
                buf_mutex.lock();
                if(rc>0)
                {   //每次读取都更新缓冲区
                    ModbusBufferWrite(slave, func, addr, nb, data);
                    miss = false;
                }
                else if(rc == -1)
                {   //设置查询失败
                    ModbusBufferSetMiss(slave, func, addr, nb);
                    miss = true;
                }
                buf_mutex.unlock();
            }
            else if (func == 0x06)
            {   //寄存器写
                log("write register", 1);
                rc = modbus_write_register(rtu_handler, addr, nb); //写入内容为nb
                if(rc>0) miss = false;
                else miss = true;
                buf_mutex.lock();
                ModbusBufferWriteByte(slave, 0x03, addr, nb);
                buf_mutex.unlock();
            }
            else if(func == 0x10)
            {   //多寄存器写
                log("write multi registers", 1);
                rc = modbus_write_registers(rtu_handler, addr, nb, wdata);
                if(rc>0) miss = false;
                else miss = true;
                buf_mutex.lock();
                ModbusBufferWrite(slave, 0x03, addr, nb, wdata);
                buf_mutex.unlock();
            }
            if (currenttask.feedback)
            {   //需要反馈消息给TCP
                response_mutex.lock();
                responsedata.enable = true;
                responsedata.miss = miss;
                response_mutex.unlock();
            }
            send = false;
        }
        timespec t;
        t.tv_sec = 0;t.tv_nsec = tick_ms*1000*1000;
        nanosleep(&t,NULL);
    }
    return 0;
}
